COMMENT ⊗   VALID 00006 PAGES
C REC  PAGE   DESCRIPTION
C00001 00001
C00002 00002	 POINTY and AL display routines and initialization
C00005 00003		initialization routine	UPDINIT
C00007 00004		routine to print out joint value PRTW
C00011 00005		routine to read joint angles every 20 msec DOWHER
C00013 00006	LOCAL STORAGE AREA
C00016 ENDMK
C⊗;
COMMENT ⊗ POINTY and AL display routines and initialization
	⊗;
TTTTRANS==1


DATA
TRNADR:	.BLKW	60
CODE

;DEVICE MECHANISM BITS

YELARM== 1	;MECHANISM BITS: YELLOW ARM, NOT INCLUDING HAND
YELHND== 2 	;YELLOW HAND
BLUARM== 4	;BLUE ARM, NOT INCLUDING HAND
BLUHND== 10	;BLUE HAND
BTABOFF==14.	;OFFSET FOR POINTERS INTO TABLES

COMMENT ⊗
	The following are the character codes for the VT05


	HOME POSITION		35	..use with fillers, e.g. 10
		Move curson to top line, first character pos
	ERASE LINE		36
		... to the end of the line from the current cursor pos
	ERASE SCREEN		37	.. use with fillers
		... to end of screen from current pos
	LINE FEED		12
		... if on last line, cause scroll
	BELL			 7
	BACKSPACE		10??	..(not delete)
	CR			15
	C(→) cursor right	30
	C(←) cursor left	10
	C(↑) curson up		32
	C(↓) cursor down(one line)13
	CAD			16	.. USE FILLER
		.. direct cursor addressing
		.. next two words(bytes?) represent
			y-addres or line location
			x-address of char location
				(except that octal 40 - 1 must be added
				if you assume that lines begin 
				at 1 instead of 0)
⊗;


HOME==35
EOL==36
EOS==37
CR==15
LF==12
BELL==7
BS==10
CRR==30
CRL==10
CRU==32
CRD==13
CAD==16
COFF==37
CFILL==10		;Used as filler because 0 will be treated as end of string
COMMA==',
ALHOMY==COFF+14		;Ycoord code for beginning of AL display section
JTHOMY==COFF+7		;Ycoord code for beginning of joint display line
ADHOMY==COFF+10		;Ycoord code for beginning of A-D reading display line
EUHOMY==COFF+12		;Ycoord code for beginning of euler angle display line

JTHOMX==COFF+17
EUHOMX==COFF+17
ALHOMX==COFF+1
ADHOMX==COFF+17
;	initialization routine	UPDINIT
;	
;	This routine initializes the updating by clearing the
;VT05 screen, and typing the header information and the titles
;and then starts up two jobs PRWT and DOWHER which continously print
;out and update the joint angle information
;	All registers are saved
;
UPDINIT: PUSH	<R0,R1,R2,R3,R4,R5>
	EVWAIT	CSLEVT
	MOV	#CLRSCR,R0	;GO TO TOP OF PAGE ON VT05 and clear VT05
	JSR	PC,PTYPSTR
	EVSIG	CSLEVT
	CLR	FRMDDT		;to clear from run to run
	CLR	DSPOK		;turn on display joint angles and trans angles
	JSR	PC,DSPINIT	;INITIALISE THE DISPLAY
	SCHEDU	#DWHRPD,#DOWHER,#USRDM,#2	;SCHEDULE JOB FOR READING ARM CONTIN
	SCHEDU	#PRTWPD,#PRTW,#USRDM,#4	;SCHEDULE JOB FOR PRINTING OUT DISPLAY
	POP	<R5,R4,R3,R2,R1,R0>
	RTS	PC

DSPINIT:EVWAIT	CSLEVT		;GRAB CONSOLE
	MOV	#HEADER,R0       ;TYPE OUT POINTY HEADER
	JSR	PC,PTYPSTR
	EVSIG	CSLEVT		;RELEASE CONSOLE
	MOV	ALHOMP,CURYXAL	;initialize position for AL display
	MOV	#8.,R0		;SET FORMAT TO PRINT OUT COLUMNS WITH WIDTH = 8
	MOV	#2,R1		;AND DIGITS = 2
	JSR	PC,FORMAT
	RTS	PC
;	routine to print out joint value PRTW


DATA
PRTWPD:	PDBLK	1,100,FP	; SET UP PBD FOR PRINTING ROUTINE
DSPOK:	0			; FLAG TO INDICATE IF DISPLAY WANTED
CODE
PRTW:	TST	ALLIVE		;TEST IF AL IS DONE
	BNE	1$
	DISMIS			;go away, AL is done
1$:	TST	DSPOK		;Test if want the display
	BEQ	2$
	SLEEP	#1000.		;No,we dont want display
	BR	PRTW		;Go check if now we want display
2$:	TST	FRMDDT		;TEST IF JUST CAME FROM DDT
	BEQ	3$
	JSR	PC,DSPINIT	;YES, SO REINITIALIZE DISPLAY
	CLR	FRMDDT		;CLEAR FROM DDT FLAG
3$:	MOV	#IOBUF,R0	;START BUILDING OUTPUT STRING IN HERE
	MOV	LTHPTR,R2	;GET POINTERS JOINT READINGS
	ADD	#BTABOFF,R2	;TO GET VALUES FOR BLUE ARM
	MOV	#7,R3		;7 JOINTS IN ALL
JTCLP:	LDF 	@(R2)+,AC0	;GET JOINT ANGLE
	JSR	PC,CVF		;CONVERT JOINT ANGLE TO STRING 
	SOB	R3,JTCLP
	CLRB	(R0)+		;END OF INFO
	EVWAIT	CSLEVT
	MOV	#JTHOME,R0
	JSR	PC,PTYPSTR	;JUMP TO LINE TO PRINT CURRENT VALUES
	MOV	#IOBUF,R0
	JSR	PC,PTYPSTR	;TYPE OUT WHOLE STRING
	EVSIG	CSLEVT
	MOV	#9.,R0		;CHANGE STRING FORMAT FOR INTEGER NUMBER
	CLR	R1
	JSR	PC,FORMAT
	MOV	#IOBUF,R0	;START BUILDING OUT STRING AGAIN
	MOV	LPOTPTR,R2	;GET POINTERS TO POTS
	ADD	#BTABOFF,R2	;TO GET FOR BLUE ARM
	MOV	#7,R3		;CONVERT THE ACTUAL POT READINGS
NEXTP:	LDCIF	@(R2),AC0	;GET A/D READING
	TST	@(R2)+		;CHECK IF ROUND UP OR DOWN
	BLT	RNDDN
	ADDF	#40000,AC0	;ROUND OFF READING
	BR	.+6
RNDDN:	SUBF	#40000,AC0
	JSR	PC,CVF		;CONVERT TO ASC
	SOB	R3,NEXTP
	JSR	PC,RSTFOR	;RESET THE FORMAT
	CLRB	(R0)		;PUT IN EMPTY BYTE
	EVWAIT	CSLEVT
	MOV	#ADHOME,R0
	JSR	PC,PTYPSTR	;JUMP TO LINE TO PRINT CURRENT VALUES
	MOV	#IOBUF,R0
	JSR	PC,PTYPSTR	;TYPE OUT WHOLE STRING
	EVSIG	CSLEVT
   .IFNZ TTTTRANS
	MOV	#TRNADR,R0	;GET CURRENT TRANSFORM IN HERE
	MOV	LTHPTR,R1	;POINT TO THE CURRENT JOINT ANGLES
	MOV	#BLUARM,R2	;USE SOLUTION FOR BLUE ARM
	JSR	PC,@LUPDATE	;COMPUTE THE CURRENT TRANSFORM
	MOV	#TRNADR,R0	;COMPUTE THE EULER ANGLES
	MOV	#EANGLE,R1	;PUT THEM IN HERE
	JSR	PC,@LEULER
	MOV	#IOBUF,R0	;CONVERT TO STRING FOR OUTPUT
	MOV	#6,R3
	MOV	#EANGLE,R2
EANGC:	LDF	(R2)+,AC0
	JSR	PC,CVF
	SOB	R3,EANGC	;REPEAT TILL DONE
	CLRB	(R0)
	EVWAIT	CSLEVT
	MOV	#EUHOME,R0
	JSR	PC,PTYPSTR	;GET TO RIGHT PLACE
	MOV	#IOBUF,R0
	JSR	PC,PTYPSTR	;TYPE OUT THE EULER ANGLES
	EVSIG	CSLEVT
   .ENDC
	SLEEP	#100		;GO SLEEP 100 MSEC TO GIVE AL INTERPRETER A CHANCE
	JMP	PRTW
;	routine to read joint angles every 20 msec DOWHER
DATA
DWHRPD:	PDBLK	2,100,FP	; SET UP PBD FOR PROCESS THAT LOOKS AT ARM POSITION
CODE
				; EVERY 20 MSEC
DOWHER:	MOV	#BARMSB+BHANDSB,WLST	;DO A WHERE FOR THE BLUE ARM
       	MOV	#WLST,R0
	MOV	#DEVICE,R1
	JSR	PC,@LWHERE	;READ THE JOINT POSITIONS
; following code commented out because it gets messed up when the
; arm is moving
;	TST	R0		;CHECK IF ANY ERRORS
;	BEQ	$1		;BRANCH IF OK
;	EVWAIT	CSLEVT
;	OUTSTR	BADW		;ELSE TYPE ERROR MESSAGE
;	JSR	PC,TYPOCT	;PRINT OUT ERROR VALUE
;	EVSIG	CSLEVT
	TST	ALLIVE		;TEST IF AL PROGRAM FINISHED
	BNE	1$
	DISMIS
1$:	SLEEP	#20.		; GO TO SLEEP FOR 20 MSECS
	BR	DOWHER

;LOCAL STORAGE AREA
DATA
DEVICE:	.BLKW	33	;DEVICE BLOCK FOR WHERE
IOBUF:	.BLKW	400.	;OUTPUT STRING BUFFER
EANGLE:	.BLKW	12.	;EULER ANGLES STORED HERE
WLST:	374    		;JOINT SERVO BIT
	0

;OUTPUT STRINGS
		; TOP OF VT05 SCREEN
CLRSCR:	.BYTE	HOME,CFILL,CFILL,CFILL,EOS,CFILL,CFILL,CFILL,0,0
HEADER:	.BYTE	HOME,CFILL,CFILL,CFILL,EOL
	.ASCII	/	       P O I N T Y   R U N T I M E   S Y S T E M/
	.BYTE	CR,LF,EOL
	.ASCII	/		  JT 1    JT 2    JT 3    JT 4    JT 5    JT 6     HAND/
	.BYTE	CR,LF,EOL
	.ASCII	/LOW STOPS	 -45.0  -165.0     6.75 -395.0   -95.0  -110.0    -0.20/
	.BYTE	CR,LF,EOL
	.ASCII	/HIGH STOPS	 190.0   -50.0    33.00  205.0    95.0   200.0     3.80/
	.BYTE	CR,LF,EOL
	.ASCII	/PARK POSITION	 180.0   -90.0    14.00  -90.0    90.0     0.0/
	.BYTE	CR,LF,EOL
	.ASCII	/BLUE ARM/
	.BYTE	CR,LF,EOL
	.ASCII	/CURRENT POS/
	.BYTE	CR,LF,EOL
	.ASCII	/A-D READINGS/
   .IFNZ TTTTRANS
	.BYTE	CR,LF,EOL
        .ASCII  /EULER ANGLES	    X       Y       Z       O       A       T/
   .ENDC
	.BYTE	CR,LF,EOL
	.BYTE	CR,LF,EOL
	.ASCII	/		   A L   R U N T I M E   S Y S T E M/
	.BYTE	EOL,CR,LF,0
	.EVEN
ALHOMP:	.BYTE	ALHOMY,ALHOMX
JTHOME:	.BYTE	CAD,JTHOMY,JTHOMX,0
ADHOME:	.BYTE	CAD,ADHOMY,ADHOMX,0
EUHOME:	.BYTE	CAD,EUHOMY,EUHOMX,0
	.EVEN

CODE